﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Testcraft.Physics
{
    class PhysicsEngine
    {
        public List<PhysicsBody> Bodies { get; private set; }
        public List<IForce> Forces {get; private set;}
        public Scene.Octree<PhysicsBody> Scene { get; set; }

        public PhysicsEngine()
        {
            this.Bodies = new List<PhysicsBody>();
            this.Forces = new List<IForce>();
        }

        public void Update(GameTime gameTime)
        {
            // Calculate net forces and new velocities
            for (int i = 0; i < this.Bodies.Count; i++)
            {
                if(!this.Bodies[i].InRest)
                {
                    this.Bodies[i].NetForces = Vector3.Zero;
                    this.Bodies[i].CommitPendingForces();
                    for(int j = 0; j < this.Forces.Count; j++)
                        this.Forces[j].ApplyOnBody(this.Bodies[i]);

                    // F = m * a;
                    // F / m = a;
                    Vector3 acceleration = this.Bodies[i].NetForces / this.Bodies[i].Weight;

                    // Scale acceleration with elapsed time
                    acceleration *= (float)gameTime.ElapsedGameTime.TotalSeconds;

                    this.Bodies[i].Velocity += acceleration;
                    float squaredLength = this.Bodies[i].Velocity.LengthSquared();
                    if (squaredLength > this.Bodies[i].VelocityCapSquared)
                    {
                        float factor = this.Bodies[i].VelocityCapSquared / squaredLength;
                        this.Bodies[i].Velocity *= factor;
                    }
                }
            }

            // Calculate new positions
            for (int i = 0; i < this.Bodies.Count; i++)
            {
                if (!this.Bodies[i].InRest)
                {
                    Vector3 newPosition = this.Bodies[i].Position + this.Bodies[i].Velocity;

                    BoundingBox translatedBounds = this.Bodies[i].Bounds;
                    translatedBounds.Max += this.Bodies[i].Velocity;
                    translatedBounds.Min += this.Bodies[i].Velocity;

                    List<PhysicsBody> intersections = this.Scene.GetIntersections(translatedBounds);

                    // DETTA ÄR SÄKERT FEL! Kan inte tänka i värmen.
                    for (int j = 0; j < intersections.Count; j++)
                    {
                        if (intersections[j] != this.Bodies[i])
                        {

                            // Calculate the box that "defines" the intersection.
                            Vector3 intersectionMinimum = Vector3.Max(translatedBounds.Min, intersections[j].Bounds.Min);
                            Vector3 intersectionMaximum = Vector3.Min(translatedBounds.Max, intersections[j].Bounds.Max);

                            Vector3 intersectionDelta = intersectionMaximum - intersectionMinimum;

                            Vector3 direction = intersections[j].Position - this.Bodies[i].Position;
                            Vector3 absoluteDirection = new Vector3(Math.Abs(direction.X), Math.Abs(direction.Y), Math.Abs(direction.Z));

                            if (absoluteDirection.X > absoluteDirection.Y && absoluteDirection.X > absoluteDirection.Z)
                            {
                                float xMovement = direction.X > 0 ? intersectionDelta.X : -intersectionDelta.X;
                                newPosition.X -= xMovement;
                                translatedBounds.Max.X -= xMovement;
                                translatedBounds.Min.X -= xMovement;
                            }
                            else if (absoluteDirection.Y > absoluteDirection.X && absoluteDirection.Y > absoluteDirection.Z)
                            {
                                float yMovement = direction.Y > 0 ? intersectionDelta.Y : -intersectionDelta.Y;
                                newPosition.Y -= yMovement;
                                translatedBounds.Max.Y -= yMovement;
                                translatedBounds.Min.Y -= yMovement;
                            }
                            else
                            {
                                float zMovement = direction.Z > 0 ? intersectionDelta.Z : -intersectionDelta.Z;
                                newPosition.Z -= zMovement;
                                translatedBounds.Max.Z -= zMovement;
                                translatedBounds.Min.Z -= zMovement;
                            }
                        }
                    }

                    this.Bodies[i].Position = newPosition;
                    this.Bodies[i].PreviousBounds = this.Bodies[i].Bounds;
                    this.Bodies[i].Bounds = translatedBounds;

                    if(this.Bodies[i].PreviousBounds.Equals(this.Bodies[i].Bounds))
                        this.Bodies[i].InRest = true;
                    
                }
            }
        }
    }
}
